/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef AGV_H
#define AGV_H

#include "Object.h"
#include "Sick.h"
#include "Shapes.h"
//#include "MLMA.h"
#include "../MaCI/interfaces/SpeedCtrl/SpeedCtrlServer.hpp"
#include "../MaCI/interfaces/SpeedCtrl/SpeedCtrlTypes.hpp"
#include "../MaCI/interfaces/Position/PositionServer.hpp"
#include "../MaCI/interfaces/Position/PositionTypes.hpp"
#include "../MaCI/interfaces/Energy/EnergyServer.hpp"
#include <string>

#ifdef MOTTI_MAP_DECODER_SUPPORT
#include "MottiMap.hpp"
#endif


#include "GLProvider.h"



#define CHASSIS_WIDTH 1.00
#define CHASSIS_HEIGHT 0.60
#define CHASSIS_LENGTH 0.40

#define FORK_LENGTH 0.90
#define FORK_WIDTH 0.15
#define FORK_HEIGHT 0.05
#define FORK_GAP 0.6

#define GROUND_CLEARANCE 0.05

#define MAX_MOTOR_SPEED 10
#define MAX_ANGULAR_SPEED 10
#define MAX_ACCELERATION 10

#define POSITION_INTERVAL 0.1
#define ENERGY_INTERVAL 1.0

#define SCANNER_OFFSET_Y 0.2
#define SCANNER_OFFSET_Z -0.25
#define LASER_INTERVAL 0.1

//class xmlNode;

class CAGV : public CObject
{
  public:
    
    CAGV(CWorld* World, float x, float y, float heading, std::string gimnetName, std::string MaCIGroup);
    
    void Activate();

    void Update(float StepSize);

    void OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2);
    
    void Draw();
    
    const float IdlePowerConsumption;
    
    const float DrivePowerConsumptionMultiplier;
    
    const float MaxBatteryLevel;
    
    static CObject* LoadMLMA(CWorld *world, xmlNode *node);
    
 private: // Variables
    gimi::GIMI gimi;
    MaCI::MaCICtrl::CMaCICtrlServer mcs;
    MaCI::SpeedCtrl::CSpeedCtrlServer SpeedCtrlServer;
    MaCI::Position::CPositionServer PositionServer;
    MaCI::Energy::CEnergyServer EnergyServer;
    
    std::string gimnetName;
    std::string MaCIGroup;
    float speed, angularSpeed, posX, posY, heading;
    float batteryLevel, powerConsumption;
    
    float positionTimer, energyTimer;
    
    CBox *chassis;
    CBox *leftFork, *rightFork;
    TMaterial chassisMaterial, forkMaterial;
    CSick *LaserScanner;


    void renderStrokeFontString(
                                float x, 
                                float y, 
                                float z, 
                                void *font, 
                                const char *string);


    CWorld* iWorld;
    CGLProvider* iGLProvider;


#ifdef MOTTI_MAP_DECODER_SUPPORT
 public:
    void SetMottiMapFilename(const std::string &aMottiMapFilename);
 private:
    CMottiMap iMottiMap;
    std::string iMottiMapFilename;
#endif
    ownTime_ms_t iLastBatteryStatusUpdate;

 private: // Functions
    void updateBattery(const float &aStepSize);
    
    void connectGIMI();
    
};

#endif
